{
  "nbformat": 4,
  "nbformat_minor": 0,
  "metadata": {
    "kernelspec": {
      "display_name": "Python 3",
      "language": "python",
      "name": "python3"
    },
    "language_info": {
      "codemirror_mode": {
        "name": "ipython",
        "version": 3
      },
      "file_extension": ".py",
      "mimetype": "text/x-python",
      "name": "python",
      "nbconvert_exporter": "python",
      "pygments_lexer": "ipython3",
      "version": "3.6.9-final"
    },
    "colab": {
      "name": "Robotic Manipulation - Motion Planning.ipynb",
      "provenance": [],
      "collapsed_sections": []
    }
  },
  "cells": [
    {
      "cell_type": "markdown",
      "metadata": {
        "id": "EgiF12Hf1Dhs"
      },
      "source": [
        "**I recommend you run the first code cell of this notebook immediately, to start provisioning drake on the cloud machine, then you can leave this window open as you [read the textbook](manipulation.csail.mit.edu/trajectories.html).**\n",
        "\n",
        "# Notebook setup\n",
        "\n",
        "The following cell will:\n",
        "- on Colab (only), install Drake to `/opt/drake`, install Drake's prerequisites via `apt`, and add pydrake to `sys.path`.  This will take approximately two minutes on the first time it runs (to provision the machine), but should only need to reinstall once every 12 hours.  If you navigate between notebooks using Colab's \"File->Open\" menu, then you can avoid provisioning a separate machine for each notebook.\n",
        "- launch a server for our 3D visualizer (MeshCat) that will be used for the remainder of this notebook.\n",
        "\n",
        "You will need to rerun this cell if you restart the kernel, but it should be fast because the machine will already have drake installed."
      ]
    },
    {
      "cell_type": "code",
      "metadata": {
        "id": "eeMrMI0-1Dhu"
      },
      "source": [
        "import importlib\n",
        "import sys\n",
        "from urllib.request import urlretrieve\n",
        "\n",
        "# Install drake.\n",
        "if 'google.colab' in sys.modules and importlib.util.find_spec('manipulation') is None:\n",
        "    urlretrieve(f\"http://manipulation.csail.mit.edu/scripts/setup/setup_manipulation_colab.py\",\n",
        "                \"setup_manipulation_colab.py\")\n",
        "    from setup_manipulation_colab import setup_manipulation\n",
        "    setup_manipulation(manipulation_sha='master', drake_version='latest', drake_build='continuous')\n",
        "\n",
        "# Determine if this notebook is currently running as a notebook or a unit test.\n",
        "from IPython import get_ipython\n",
        "running_as_notebook = get_ipython() and hasattr(get_ipython(), 'kernel')\n",
        "\n",
        "# Use pyngrok on colab.\n",
        "server_args = []\n",
        "if 'google.colab' in sys.modules:\n",
        "  server_args = ['--ngrok_http_tunnel']\n",
        "\n",
        "# Start a single meshcat server instance to use for the remainder of this notebook.\n",
        "from meshcat.servers.zmqserver import start_zmq_server_as_subprocess\n",
        "proc, zmq_url, web_url = start_zmq_server_as_subprocess(server_args=server_args)\n",
        "proc2, zmq_url2, web_url2 = start_zmq_server_as_subprocess(server_args=server_args)\n",
        "\n",
        "# Let's do all of our imports here, too.\n",
        "import meshcat\n",
        "import numpy as np\n",
        "import pydrake.all\n",
        "import os\n",
        "from IPython.display import display\n",
        "from ipywidgets import Textarea\n",
        "from functools import partial\n",
        "import mcubes\n",
        "\n",
        "from pydrake.all import RigidTransform, RotationMatrix, RollPitchYaw\n",
        "from pydrake.examples.manipulation_station import ManipulationStation\n",
        "import pydrake.multibody.jupyter_widgets\n",
        "import pydrake.systems.jupyter_widgets\n",
        "from manipulation.utils import FindResource\n",
        "from manipulation.scenarios import AddIiwa, AddPlanarIiwa, AddTwoLinkIiwa, AddWsg, AddShape\n",
        "from manipulation.jupyter_widgets import MakePoseSlidersThatPublishOnCallback"
      ],
      "execution_count": null,
      "outputs": []
    },
    {
      "source": [
        "In the cells below, I've told meshcat not to flush it's geometry everytime we run.  If you ever need to clear the geometry in the meshcat visualizer, just run this cell."
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {},
      "outputs": [],
      "source": [
        "meshcat.Visualizer(zmq_url=zmq_url).delete()"
      ]
    },
    {
      "source": [
        "# Interactive inverse kinematics\n",
        "\n",
        "This first cell gives us an interface that is very similar to the differential IK teleop interface that we used before.  See if you can spot any differences."
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {},
      "outputs": [],
      "source": [
        "def teleop_inverse_kinematics():\n",
        "    builder = pydrake.systems.framework.DiagramBuilder()\n",
        "\n",
        "    plant, scene_graph = pydrake.multibody.plant.AddMultibodyPlantSceneGraph(builder, time_step=0.001)\n",
        "    iiwa = AddIiwa(plant)\n",
        "    wsg = AddWsg(plant, iiwa, welded=True)\n",
        "    plant.Finalize()\n",
        "\n",
        "    visualizer = pydrake.systems.meshcat_visualizer.ConnectMeshcatVisualizer(\n",
        "        builder, \n",
        "        scene_graph, \n",
        "        zmq_url=zmq_url,\n",
        "        server_args=server_args,\n",
        "        delete_prefix_on_load=False)\n",
        "\n",
        "    diagram = builder.Build()\n",
        "    context = diagram.CreateDefaultContext()\n",
        "    plant_context = plant.GetMyContextFromRoot(context)\n",
        "\n",
        "    q0 = plant.GetPositions(plant_context)\n",
        "    gripper_frame = plant.GetFrameByName(\"body\", wsg)\n",
        "\n",
        "    console = Textarea(value=\"\", description=\"\", layout={'width':'60%','height':'100px'}, style={'description_width':'initial'})\n",
        "\n",
        "    def my_callback(context, pose):\n",
        "        ik = pydrake.multibody.inverse_kinematics.InverseKinematics(plant, plant_context)\n",
        "        ik.AddPositionConstraint(gripper_frame, [0, 0, 0], plant.world_frame(), pose.translation(), pose.translation())\n",
        "        ik.AddOrientationConstraint(gripper_frame, RotationMatrix(), plant.world_frame(), pose.rotation(), 0.0)\n",
        "        prog = ik.get_mutable_prog()\n",
        "        q = ik.q()\n",
        "        prog.AddQuadraticErrorCost(np.identity(len(q)), q0, q)\n",
        "        prog.SetInitialGuess(q, q0)\n",
        "        result = pydrake.solvers.mathematicalprogram.Solve(ik.prog())\n",
        "        X_WG = plant.EvalBodyPoseInWorld(plant_context, plant.GetBodyByName(\"body\", wsg))\n",
        "        if not result.is_success():\n",
        "            console.value += \"IK failed\\n\"\n",
        "\n",
        "    visualizer.load()\n",
        "    X_WG = plant.EvalBodyPoseInWorld(plant_context, plant.GetBodyByName(\"body\", wsg))\n",
        "    MakePoseSlidersThatPublishOnCallback(visualizer, context, my_callback, value=X_WG)\n",
        "    display(console)\n",
        "\n",
        "teleop_inverse_kinematics()\n"
      ]
    },
    {
      "source": [
        "This one has a collision to avoid.  Try moving it in negative y."
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {},
      "outputs": [],
      "source": [
        "meshcat.Visualizer(zmq_url=zmq_url).delete()"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {},
      "outputs": [],
      "source": [
        "def teleop_inverse_kinematics():\n",
        "    builder = pydrake.systems.framework.DiagramBuilder()\n",
        "\n",
        "    plant, scene_graph = pydrake.multibody.plant.AddMultibodyPlantSceneGraph(builder, time_step=0.001)\n",
        "    iiwa = AddIiwa(plant, \"with_box_collision\")\n",
        "    wsg = AddWsg(plant, iiwa, welded=True)\n",
        "    box = AddShape(plant, pydrake.geometry.Box(0.1, 0.1, 1.0), \"box\")\n",
        "    plant.WeldFrames(plant.world_frame(), plant.GetFrameByName(\"box\", box), RigidTransform([0.25, 0.0, 0.5]))\n",
        "    plant.Finalize()\n",
        "\n",
        "    visualizer = pydrake.systems.meshcat_visualizer.ConnectMeshcatVisualizer(\n",
        "        builder, \n",
        "        scene_graph, \n",
        "        zmq_url=zmq_url,\n",
        "        server_args=server_args,\n",
        "        delete_prefix_on_load=False)\n",
        "\n",
        "    diagram = builder.Build()\n",
        "    context = diagram.CreateDefaultContext()\n",
        "    plant_context = plant.GetMyContextFromRoot(context)\n",
        "\n",
        "    q0 = plant.GetPositions(plant_context)\n",
        "    gripper_frame = plant.GetFrameByName(\"body\", wsg)\n",
        "\n",
        "    console = Textarea(value=\"\", description=\"\", layout={'width':'60%','height':'100px'}, style={'description_width':'initial'})\n",
        "\n",
        "    def my_callback(context, pose):\n",
        "        ik = pydrake.multibody.inverse_kinematics.InverseKinematics(plant, plant_context)\n",
        "        ik.AddPositionConstraint(gripper_frame, [0, 0, 0], plant.world_frame(), pose.translation(), pose.translation())\n",
        "        ik.AddOrientationConstraint(gripper_frame, RotationMatrix(), plant.world_frame(), pose.rotation(), 0.0)\n",
        "        ik.AddMinimumDistanceConstraint(0.001, 0.1)\n",
        "        prog = ik.get_mutable_prog()\n",
        "        q = ik.q()\n",
        "        prog.AddQuadraticErrorCost(np.identity(len(q)), q0, q)\n",
        "        prog.SetInitialGuess(q, q0)\n",
        "        result = pydrake.solvers.mathematicalprogram.Solve(ik.prog())\n",
        "        X_WG = plant.EvalBodyPoseInWorld(plant_context, plant.GetBodyByName(\"body\", wsg))\n",
        "        if not result.is_success():\n",
        "            console.value += \"IK failed\\n\"\n",
        "\n",
        "    visualizer.load()\n",
        "    X_WG = plant.EvalBodyPoseInWorld(plant_context, plant.GetBodyByName(\"body\", wsg))\n",
        "    s = MakePoseSlidersThatPublishOnCallback(visualizer, context, my_callback, value=X_WG)\n",
        "    s[4].value = -0.2\n",
        "    s[5].value = 0.35  # set the initial z lower, to make the interaction interesting.\n",
        "    display(console)\n",
        "\n",
        "teleop_inverse_kinematics()\n"
      ]
    },
    {
      "source": [
        "This one has the hand tracking a cylinder, but is allowed to touch anywhere along the cylinder.  The sliders are controlling the pose of the cylinder. Or you can set `grasp_cylinder` to `False` and just chase the robot around with a stick."
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {},
      "outputs": [],
      "source": [
        "meshcat.Visualizer(zmq_url=zmq_url).delete()"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {},
      "outputs": [],
      "source": [
        "def teleop_inverse_kinematics(grasp_cylinder=True):\n",
        "    builder = pydrake.systems.framework.DiagramBuilder()\n",
        "\n",
        "    plant, scene_graph = pydrake.multibody.plant.AddMultibodyPlantSceneGraph(builder, time_step=0.001)\n",
        "    iiwa = AddIiwa(plant, \"with_box_collision\")\n",
        "    wsg = AddWsg(plant, iiwa, welded=True)\n",
        "    cylinder = AddShape(plant, pydrake.geometry.Cylinder(0.02, 1.0), \"cylinder\")\n",
        "    plant.Finalize()\n",
        "\n",
        "    visualizer = pydrake.systems.meshcat_visualizer.ConnectMeshcatVisualizer(\n",
        "        builder, \n",
        "        scene_graph, \n",
        "        zmq_url=zmq_url,\n",
        "        server_args=server_args,\n",
        "        delete_prefix_on_load=False)\n",
        "\n",
        "    diagram = builder.Build()\n",
        "    context = diagram.CreateDefaultContext()\n",
        "    plant_context = plant.GetMyContextFromRoot(context)\n",
        "\n",
        "    q0 = plant.GetPositions(plant_context)\n",
        "    gripper_frame = plant.GetFrameByName(\"body\", wsg)\n",
        "    cylinder_body = plant.GetBodyByName(\"cylinder\", cylinder)\n",
        "    cylinder_frame = plant.GetFrameByName(\"cylinder\", cylinder)\n",
        "\n",
        "    console = Textarea(value=\"\", description=\"\", layout={'width':'60%','height':'100px'}, style={'description_width':'initial'})\n",
        "\n",
        "    def my_callback(context, pose):\n",
        "        ik = pydrake.multibody.inverse_kinematics.InverseKinematics(plant, plant_context)\n",
        "        ik.AddPositionConstraint(cylinder_frame, [0, 0, 0], plant.world_frame(), pose.translation(), pose.translation())\n",
        "        ik.AddOrientationConstraint(cylinder_frame, RotationMatrix(), plant.world_frame(), pose.rotation(), 0.0)\n",
        "        if grasp_cylinder:\n",
        "            ik.AddPositionConstraint(gripper_frame, [0, 0.1, -0.02], cylinder_frame, [0, 0, -0.5], [0, 0, 0.5])\n",
        "            ik.AddPositionConstraint(gripper_frame, [0, 0.1, 0.02], cylinder_frame, [0, 0, -0.5], [0, 0, 0.5])\n",
        "        else:\n",
        "            ik.AddMinimumDistanceConstraint(0.001, 0.1)\n",
        "        prog = ik.get_mutable_prog()\n",
        "        q = ik.q()\n",
        "        prog.AddQuadraticErrorCost(np.identity(len(q)), q0, q)\n",
        "        prog.SetInitialGuess(q, q0)\n",
        "        result = pydrake.solvers.mathematicalprogram.Solve(ik.prog())\n",
        "        if not result.is_success():\n",
        "            console.value += \"IK failed\\n\"\n",
        "\n",
        "    visualizer.load()\n",
        "    X_WC = RigidTransform(RollPitchYaw(np.pi/2.0, 0, 0), [0.5, 0, 0.5])\n",
        "    s = MakePoseSlidersThatPublishOnCallback(visualizer, context, my_callback, value=X_WC)\n",
        "    display(console)\n",
        "\n",
        "# Set grasp_cylinder=False if you just want to antagonize the robot with a stick.\n",
        "teleop_inverse_kinematics(grasp_cylinder=True)\n"
      ]
    },
    {
      "source": [
        "# Visualizing the configuration space\n",
        "\n",
        "Note that I'm creating a second meshcat window for this visualization."
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {},
      "outputs": [],
      "source": [
        "meshcat.Visualizer(zmq_url=zmq_url).delete()"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {},
      "outputs": [],
      "source": [
        "def draw_configuration_space(shelves=True):\n",
        "    builder = pydrake.systems.framework.DiagramBuilder()\n",
        "\n",
        "    plant, scene_graph = pydrake.multibody.plant.AddMultibodyPlantSceneGraph(builder, time_step=0.001)\n",
        "    iiwa = AddPlanarIiwa(plant)\n",
        "    wsg = AddWsg(plant, iiwa, roll=0.0, welded=True)\n",
        "    sphere = AddShape(plant, pydrake.geometry.Sphere(0.02), \"sphere\")\n",
        "    X_WO = RigidTransform([0.6, 0, 0.65])\n",
        "    plant.WeldFrames(plant.world_frame(), plant.GetFrameByName(\"sphere\"), X_WO)\n",
        "\n",
        "    if shelves:\n",
        "        parser = pydrake.multibody.parsing.Parser(plant)\n",
        "        bin = parser.AddModelFromFile(FindResource(\"models/shelves.sdf\"))\n",
        "        plant.WeldFrames(plant.world_frame(), plant.GetFrameByName(\"shelves_body\", bin), RigidTransform([0.6,0,0.4]))\n",
        "\n",
        "    plant.Finalize()\n",
        "\n",
        "    visualizer = pydrake.systems.meshcat_visualizer.ConnectMeshcatVisualizer(\n",
        "        builder, \n",
        "        scene_graph, \n",
        "        zmq_url=zmq_url,\n",
        "        server_args=server_args,\n",
        "        delete_prefix_on_load=False)\n",
        "\n",
        "    diagram = builder.Build()\n",
        "    visualizer.load()\n",
        "    context = diagram.CreateDefaultContext()\n",
        "    plant_context = plant.GetMyContextFromRoot(context)\n",
        "\n",
        "    q0 = plant.GetPositions(plant_context)\n",
        "    gripper_frame = plant.GetFrameByName(\"body\", wsg)\n",
        "    sphere_frame = plant.GetFrameByName(\"sphere\", sphere)\n",
        "\n",
        "    ik = pydrake.multibody.inverse_kinematics.InverseKinematics(plant, plant_context)\n",
        "    collision_constraint = ik.AddMinimumDistanceConstraint(0.001, 0.01)\n",
        "    grasp_constraint = ik.AddPositionConstraint(gripper_frame, [0, 0.1, 0], sphere_frame, [0, 0, 0], [0, 0, 0])\n",
        "    \n",
        "    prog = ik.get_mutable_prog()\n",
        "    q = ik.q()\n",
        "    prog.SetInitialGuess(q, q0)\n",
        "    result = pydrake.solvers.mathematicalprogram.Solve(ik.prog())\n",
        "    if not result.is_success():\n",
        "        console.value += \"IK failed\\n\"\n",
        "\n",
        "    diagram.Publish(context)\n",
        "\n",
        "    def eval(q0, q1, q2, c, tol):\n",
        "        return float(c.evaluator().CheckSatisfied([q0, q1, q2], tol))\n",
        "\n",
        "    v = meshcat.Visualizer(zmq_url=zmq_url2)\n",
        "    v[\"/Background\"].set_property(\"visible\", False)\n",
        "    v[\"initial_guess\"].set_object(meshcat.geometry.Sphere(0.05), meshcat.geometry.MeshLambertMaterial(color=0x66ffff))\n",
        "    v[\"initial_guess\"].set_transform(meshcat.transformations.translation_matrix(q0))\n",
        "    v[\"ik_solution\"].set_object(meshcat.geometry.Sphere(0.05), meshcat.geometry.MeshLambertMaterial(color=0x6666ff))\n",
        "    v[\"ik_solution\"].set_transform(meshcat.transformations.translation_matrix(result.GetSolution(q)))\n",
        "\n",
        "    low = plant.GetPositionLowerLimits()\n",
        "    up = plant.GetPositionUpperLimits()\n",
        "    N = 70 if running_as_notebook else 2\n",
        "    vertices, triangles = mcubes.marching_cubes_func(tuple(low), tuple(up), N, N, N, \n",
        "                                                     partial(eval, c=grasp_constraint, tol=0.05), 0.5)\n",
        "    v[\"grasp_constraint\"].set_object(meshcat.geometry.TriangularMeshGeometry(vertices, triangles),\n",
        "                                     meshcat.geometry.MeshLambertMaterial(color=0x88ee88, wireframe=False))\n",
        "\n",
        "    if shelves:\n",
        "        vertices, triangles = mcubes.marching_cubes_func(tuple(low), tuple(up), N, N, N, \n",
        "                                                         partial(eval, c=collision_constraint, tol=0.0), 0.5)\n",
        "        v[\"collision_constraint\"].set_object(meshcat.geometry.TriangularMeshGeometry(vertices, triangles),\n",
        "                                        meshcat.geometry.MeshLambertMaterial(color=0xee8888, wireframe=False))\n",
        "\n",
        "\n",
        "draw_configuration_space()"
      ]
    },
    {
      "source": [
        "# Visualizing the costs and constraints\n",
        "\n",
        "Here is another view.  Notice that at the optimal solution of the iiwa reaching into the shelf, the last joint was almost zero.  I've gone ahead and welded it to zero, so that we are now down to just a two degree of freedom IK problem.  Now we can plot the entire cost and constraint landscape.  Let's do it.\n",
        "\n",
        "There is a lot going on in the second meshcat window.  Use the controls to turn on and off different costs/constraints.  The constraints are blue where they are feasible and red where they are infeasible.  Which constraint is the horribly ugly one?"
      ],
      "cell_type": "markdown",
      "metadata": {}
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {},
      "outputs": [],
      "source": [
        "from manipulation.meshcat_utils import plot_mathematical_program\n",
        "\n",
        "def draw_ik_prog(zoom=True):\n",
        "    builder = pydrake.systems.framework.DiagramBuilder()\n",
        "\n",
        "    plant, scene_graph = pydrake.multibody.plant.AddMultibodyPlantSceneGraph(builder, time_step=0.001)\n",
        "    iiwa = AddTwoLinkIiwa(plant)\n",
        "    wsg = AddWsg(plant, iiwa, roll=0.0, welded=True)\n",
        "    sphere = AddShape(plant, pydrake.geometry.Sphere(0.02), \"sphere\")\n",
        "    X_WO = RigidTransform([0.6, 0, 0.65])\n",
        "    plant.WeldFrames(plant.world_frame(), plant.GetFrameByName(\"sphere\"), X_WO)\n",
        "\n",
        "    parser = pydrake.multibody.parsing.Parser(plant)\n",
        "    bin = parser.AddModelFromFile(FindResource(\"models/shelves.sdf\"))\n",
        "    plant.WeldFrames(plant.world_frame(), plant.GetFrameByName(\"shelves_body\", bin), RigidTransform([0.6,0,0.4]))\n",
        "\n",
        "    plant.Finalize()\n",
        "\n",
        "    visualizer = pydrake.systems.meshcat_visualizer.ConnectMeshcatVisualizer(\n",
        "        builder, \n",
        "        scene_graph, \n",
        "        zmq_url=zmq_url,\n",
        "        server_args=server_args,\n",
        "        delete_prefix_on_load=False)\n",
        "\n",
        "    diagram = builder.Build()\n",
        "    visualizer.load()\n",
        "    context = diagram.CreateDefaultContext()\n",
        "    plant_context = plant.GetMyContextFromRoot(context)\n",
        "\n",
        "    q0 = plant.GetPositions(plant_context)\n",
        "    gripper_frame = plant.GetFrameByName(\"body\", wsg)\n",
        "    sphere_frame = plant.GetFrameByName(\"sphere\", sphere)\n",
        "\n",
        "    ik = pydrake.multibody.inverse_kinematics.InverseKinematics(plant, plant_context)\n",
        "    collision_constraint = ik.AddMinimumDistanceConstraint(0.001, 0.1)\n",
        "    grasp_constraint = ik.AddPositionConstraint(gripper_frame, [0, 0.1, 0], sphere_frame, \n",
        "                                                [-0.001, -0.001, -0.001], [0.001, 0.001, 0.001])\n",
        "    \n",
        "    prog = ik.get_mutable_prog()\n",
        "    q = ik.q()\n",
        "    prog.AddQuadraticErrorCost(np.identity(len(q)), q0, q)\n",
        "    prog.SetInitialGuess(q, q0)\n",
        "    result = pydrake.solvers.mathematicalprogram.Solve(ik.prog())\n",
        "    if not result.is_success():\n",
        "        print(\"IK failed\")\n",
        "\n",
        "    diagram.Publish(context)\n",
        "\n",
        "    v = meshcat.Visualizer(zmq_url=zmq_url2)\n",
        "    v.delete()\n",
        "    v[\"/Background\"].set_property(\"visible\", False)\n",
        "    if zoom:\n",
        "        qstar = result.GetSolution(q)\n",
        "        X, Y = np.meshgrid(np.linspace(qstar[0]-0.2, qstar[0]+0.2, 75), np.linspace(qstar[1]-0.2, qstar[1]+0.2, 75))\n",
        "        point_size=0.01\n",
        "    else:        \n",
        "        low = plant.GetPositionLowerLimits()\n",
        "        up = plant.GetPositionUpperLimits()\n",
        "        X, Y = np.meshgrid(np.linspace(low[0], up[0], 175), np.linspace(low[1], up[1], 175))\n",
        "        point_size=0.05\n",
        "    plot_mathematical_program(v[\"ik\"], prog, X, Y, result, point_size=point_size)\n",
        "    \n",
        "draw_ik_prog(zoom=True)"
      ]
    },
    {
      "cell_type": "code",
      "execution_count": null,
      "metadata": {},
      "outputs": [],
      "source": []
    }
  ]
}